/**
  ****************************(C) COPYRIGHT 2019 DJI****************************
  * @file       can_receive.c/h
  * @brief      there is CAN interrupt function  to receive motor data,
  *             and CAN send function to send motor current to control motor.
  *             锟斤拷锟斤拷锟斤拷CAN锟叫断斤拷锟秸猴拷锟斤拷锟斤拷锟斤拷锟秸碉拷锟斤拷锟斤拷锟�,CAN锟斤拷锟酵猴拷锟斤拷锟斤拷锟酵碉拷锟斤拷锟斤拷锟斤拷锟斤拷频锟斤拷.
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. done
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2019 DJI****************************
  */

#include "CAN_receive.h"
#include "main.h"
#include "stdio.h"
#include "math.h"

extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;

CAN_DataTypeConvert CAN_Data;
// motor data read
void get_motor_measure(Motor_Measure_T *ptr, uint8_t data[])
{
  (ptr)->last_angle = (ptr)->angle;
  (ptr)->angle = (int16_t)((data)[0] << 8 | (data)[1]);
  (ptr)->speed_rpm = (int16_t)((data)[2] << 8 | (data)[3]);
  (ptr)->given_current = (int16_t)((data)[4] << 8 | (data)[5]);
  (ptr)->temperate = (int8_t)((data)[6]);
  (ptr)->offset_angle = (ptr)->angle - (ptr)->last_angle;
  if (fabs((float)(ptr)->offset_angle) > 6553)
  {
    (ptr)->round_cnt = ((ptr)->offset_angle > 0) ? (--(ptr)->round_cnt) : (++(ptr)->round_cnt);
  }

  (ptr)->speed = (ptr)->speed_rpm;
  (ptr)->Angle_actual = (ptr)->round_cnt * 8192 + (ptr)->angle;
}

/*
motor data,  0:chassis motor1 3508;1:chassis motor3 3508;2:chassis motor3 3508;3:chassis motor4 3508;
4:yaw gimbal motor 6020;5:pitch gimbal motor 6020;6:trigger motor 2006;
锟斤拷锟斤拷锟斤拷锟�, 0:锟斤拷锟教碉拷锟�1 3508锟斤拷锟�,  1:锟斤拷锟教碉拷锟�2 3508锟斤拷锟�,2:锟斤拷锟教碉拷锟�3 3508锟斤拷锟�,3:锟斤拷锟教碉拷锟�4 3508锟斤拷锟�;
4:yaw锟斤拷台锟斤拷锟� 6020锟斤拷锟�; 5:pitch锟斤拷台锟斤拷锟� 6020锟斤拷锟�; 6:锟斤拷锟斤拷锟斤拷锟� 2006锟斤拷锟�*/
Motor_Measure_T motor_chassis[8];

// static CAN_TxHeaderTypeDef  gimbal_tx_message;
// static uint8_t              gimbal_can_send_data[8];
static CAN_TxHeaderTypeDef chassis_tx_message;
static uint8_t chassis_can_send_data[8];
static CAN_TxHeaderTypeDef tx_message;
static uint8_t can_send_data[8];

/**
 * @brief          hal CAN fifo call back, receive motor data
 * @param[in]      hcan, the point to CAN handle
 * @retval         none
 */
/**
 * @brief          hal锟斤拷CAN锟截碉拷锟斤拷锟斤拷,锟斤拷锟秸碉拷锟斤拷锟斤拷锟�
 * @param[in]      hcan:CAN锟斤拷锟街革拷锟�
 * @retval         none
 */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
  CAN_RxHeaderTypeDef rx_header;
  uint8_t rx_data[8];
  if (hcan == &hcan1)
  {
    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);

       switch (rx_header.StdId)
       {
       case CAN_Pitch_6020_M1_ID: // 鏈粡鍑忛€熺鐨勮浆閫熶负800r/min
       {
         get_motor_measure(&motor_chassis[0], rx_data);

         break;
       }
       case CAN_FIRE_3508_M1_ID:
       {
         get_motor_measure(&motor_chassis[1], rx_data);

         break;
       }
       case CAN_FIRE_3508_M2_ID:
       {
         get_motor_measure(&motor_chassis[2], rx_data);

         break;
       }

       default:
       {
         break;
       }
       }
  }
  else if (hcan == &hcan2)
  {
    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);

    switch (rx_header.StdId)
    {
    case BoardA_Message_ID:
      get_BoardA_message(rx_data);
      break;

    default:
    {
      break;
    }
    }
  }
}
extern QueueHandle_t Board_QueueHandle;
float Can_RxData[2] = {0};
// 鑾峰彇A鏉夸俊鎭�
void get_BoardA_message(uint8_t *rxdata)
{
  // uint8_t i = 0;
  // for (i = 0; i < 4; i++)
  // {
  //   CAN_Data.udata[i] = rxdata[i];
  // }
  // Can_RxData[0] = CAN_Data.fdata;
  // i = 0;
  // for (i = 0; i < 4; i++)
  // {
  //   CAN_Data.udata[i] = rxdata[i+4];
  // }
  // Can_RxData[1] = CAN_Data.fdata;
  Can_RxData[0] = *((float *)&rxdata[0]);
  Can_RxData[1] = *((float *)&rxdata[4]);
  xQueueSendFromISR(Board_QueueHandle, Can_RxData, pdTRUE);
}

void CAN_Send_BoardA(uint8_t *data)
{
  uint32_t send_mail_box;
  tx_message.StdId = SendMsg_to_BoardA_ID;
  tx_message.IDE = CAN_ID_STD;
  tx_message.RTR = CAN_RTR_DATA;
  tx_message.DLC = 0x08; // 一锟斤拷锟斤拷锟斤拷帧锟斤拷锟�8锟街斤拷
  HAL_CAN_AddTxMessage(&GIMBAL_CAN, &tx_message, data, &send_mail_box);
}

/**
 * @brief          send control current of motor (0x205, 0x206, 0x207, 0x208)
 * @param[in]      yaw: (0x205) 6020 motor control current, range [-30000,30000]
 * @param[in]      pitch: (0x206) 6020 motor control current, range [-30000,30000]
 * @param[in]      shoot: (0x207) 2006 motor control current, range [-10000,10000]
 * @param[in]      rev: (0x208) reserve motor control current
 * @retval         none
 */
/**
 * @brief          锟斤拷锟酵碉拷锟斤拷锟斤拷频锟斤拷锟�(0x205,0x206,0x207,0x208)
 * @param[in]      yaw: (0x205) 6020锟斤拷锟斤拷锟斤拷频锟斤拷锟�, 锟斤拷围 [-30000,30000]
 * @param[in]      pitch: (0x206) 6020锟斤拷锟斤拷锟斤拷频锟斤拷锟�, 锟斤拷围 [-30000,30000]
 * @param[in]      shoot: (0x207) 2006锟斤拷锟斤拷锟斤拷频锟斤拷锟�, 锟斤拷围 [-10000,10000]
 * @param[in]      rev: (0x208) 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷频锟斤拷锟�
 * @retval         none
 */
// void CAN_cmd_gimbal(int16_t yaw, int16_t pitch, int16_t shoot, int16_t rev)
//{
//     uint32_t send_mail_box;
//     gimbal_tx_message.StdId = CAN_GIMBAL_ALL_ID;
//     gimbal_tx_message.IDE = CAN_ID_STD;
//     gimbal_tx_message.RTR = CAN_RTR_DATA;
//     gimbal_tx_message.DLC = 0x08;
//     gimbal_can_send_data[0] = (yaw >> 8);
//     gimbal_can_send_data[1] = yaw;
//     gimbal_can_send_data[2] = (pitch >> 8);
//     gimbal_can_send_data[3] = pitch;
//     gimbal_can_send_data[4] = (shoot >> 8);
//     gimbal_can_send_data[5] = shoot;
//     gimbal_can_send_data[6] = (rev >> 8);
//     gimbal_can_send_data[7] = rev;
//     HAL_CAN_AddTxMessage(&GIMBAL_CAN, &gimbal_tx_message, gimbal_can_send_data, &send_mail_box);
// }

/**
 * @brief          send CAN packet of ID 0x700, it will set chassis motor 3508 to quick ID setting
 * @param[in]      none
 * @retval         none
 */
/**
 * @brief          锟斤拷锟斤拷ID为0x700锟斤拷CAN锟斤拷,锟斤拷锟斤拷锟斤拷锟斤拷3508锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷ID
 * @param[in]      none
 * @retval         none
 */
void CAN_cmd_chassis_reset_ID(void)
{
  uint32_t send_mail_box;
  chassis_tx_message.StdId = 0x700;
  chassis_tx_message.IDE = CAN_ID_STD;
  chassis_tx_message.RTR = CAN_RTR_DATA;
  chassis_tx_message.DLC = 0x08;
  chassis_can_send_data[0] = 0;
  chassis_can_send_data[1] = 0;
  chassis_can_send_data[2] = 0;
  chassis_can_send_data[3] = 0;
  chassis_can_send_data[4] = 0;
  chassis_can_send_data[5] = 0;
  chassis_can_send_data[6] = 0;
  chassis_can_send_data[7] = 0;

  HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

/**
 * @brief          send control current of motor (0x201, 0x202, 0x203, 0x204)
 * @param[in]      motor1: (0x201) 3508 motor control current, range [-16384,16384]
 * @param[in]      motor2: (0x202) 3508 motor control current, range [-16384,16384]
 * @param[in]      motor3: (0x203) 3508 motor control current, range [-16384,16384]
 * @param[in]      motor4: (0x204) 3508 motor control current, range [-16384,16384]
 * @retval         none
 */
/**
 * @brief          锟斤拷锟酵碉拷锟斤拷锟斤拷频锟斤拷锟�(0x201,0x202,0x203,0x204)
 * @param[in]      motor1: (0x201) 3508锟斤拷锟斤拷锟斤拷频锟斤拷锟�, 锟斤拷围 [-16384,16384]
 * @param[in]      motor2: (0x202) 3508锟斤拷锟斤拷锟斤拷频锟斤拷锟�, 锟斤拷围 [-16384,16384]
 * @param[in]      motor3: (0x203) 3508锟斤拷锟斤拷锟斤拷频锟斤拷锟�, 锟斤拷围 [-16384,16384]
 * @param[in]      motor4: (0x204) 3508锟斤拷锟斤拷锟斤拷频锟斤拷锟�, 锟斤拷围 [-16384,16384]
 * @retval         none
 */
// void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4, int16_t motor5, int16_t motor6, int16_t motor7, int16_t motor8)
//{
//     uint32_t send_mail_box  ;
//     chassis_tx_message.StdId = CAN_CHASSIS_6020_ID;
//     chassis_tx_message.IDE = CAN_ID_STD;
//     chassis_tx_message.RTR = CAN_RTR_DATA;
//     chassis_tx_message.DLC = 0x08;
//     chassis_can_send_data[0] = motor1 >> 8;
//     chassis_can_send_data[1] = motor1;
//     chassis_can_send_data[2] = motor2 >> 8;
//     chassis_can_send_data[3] = motor2;
//     chassis_can_send_data[4] = motor3 >> 8;
//     chassis_can_send_data[5] = motor3;
//     chassis_can_send_data[6] = motor4 >> 8;
//     chassis_can_send_data[7] = motor4;
//     HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);

//    chassis_tx_message.StdId = CAN_CHASSIS_B4_ID;
//    chassis_tx_message.IDE = CAN_ID_STD;
//    chassis_tx_message.RTR = CAN_RTR_DATA;
//    chassis_tx_message.DLC = 0x08;
//    chassis_can_send_data[0] = motor5 >> 8;
//    chassis_can_send_data[1] = motor5;
//    chassis_can_send_data[2] = motor6 >> 8;
//    chassis_can_send_data[3] = motor6;
//    chassis_can_send_data[4] = motor7 >> 8;
//    chassis_can_send_data[5] = motor7;
//    chassis_can_send_data[6] = motor8 >> 8;
//    chassis_can_send_data[7] = motor8;

//    HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
//}

void CAN_Send_Pitch_cmd(int16_t motor1)
{
  // Pitch浜戝彴 鐢靛帇缁欏畾鍊艰寖鍥达細-25000~0~25000
  uint32_t send_mail_box;
  chassis_tx_message.StdId = CAN_Pitch_6020_ID;
  chassis_tx_message.IDE = CAN_ID_STD;
  chassis_tx_message.RTR = CAN_RTR_DATA;
  chassis_tx_message.DLC = 0x08;
  chassis_can_send_data[0] = 0;
  chassis_can_send_data[1] = 0;
  chassis_can_send_data[2] = motor1 >> 8;
  chassis_can_send_data[3] = motor1;
  chassis_can_send_data[4] = 0;
  chassis_can_send_data[5] = 0;
  chassis_can_send_data[6] = 0;
  chassis_can_send_data[7] = 0;
  HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

void CAN_Send_Fire_cmd(int16_t motor1, int16_t motor2)
{

  // 寮€鐏�3508鐢垫満
  uint32_t send_mail_box;
  chassis_tx_message.StdId = CAN_FIRE_3508_ID;
  chassis_tx_message.IDE = CAN_ID_STD;
  chassis_tx_message.RTR = CAN_RTR_DATA;
  chassis_tx_message.DLC = 0x08;
  chassis_can_send_data[0] = 0;
  chassis_can_send_data[1] = 0;
  chassis_can_send_data[2] = motor1 >> 8;
  chassis_can_send_data[3] = motor1;
  chassis_can_send_data[4] = motor2 >> 8;
  chassis_can_send_data[5] = motor2;
  chassis_can_send_data[6] = 0;
  chassis_can_send_data[7] = 0;
  HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

/**
 * @brief          return the yaw 6020 motor data point
 * @param[in]      none
 * @retval         motor data point
 */
/**
 * @brief          锟斤拷锟斤拷yaw 6020锟斤拷锟斤拷锟斤拷锟街革拷锟�
 * @param[in]      none
 * @retval         锟斤拷锟斤拷锟斤拷锟街革拷锟�
 */
// const motor_measure_t *get_yaw_gimbal_motor_measure_point(void)
//{
//     return &motor_chassis[4];
// }

/**
 * @brief          return the pitch 6020 motor data point
 * @param[in]      none
 * @retval         motor data point
 */
/**
 * @brief          锟斤拷锟斤拷pitch 6020锟斤拷锟斤拷锟斤拷锟街革拷锟�
 * @param[in]      none
 * @retval         锟斤拷锟斤拷锟斤拷锟街革拷锟�
 */
// const motor_measure_t *get_pitch_gimbal_motor_measure_point(void)
//{
//     return &motor_chassis[5];
// }

/**
 * @brief          return the trigger 2006 motor data point
 * @param[in]      none
 * @retval         motor data point
 */
/**
 * @brief          锟斤拷锟截诧拷锟斤拷锟斤拷锟� 2006锟斤拷锟斤拷锟斤拷锟街革拷锟�
 * @param[in]      none
 * @retval         锟斤拷锟斤拷锟斤拷锟街革拷锟�
 */
// const motor_measure_t *get_trigger_motor_measure_point(void)
//{
//     return &motor_chassis[6];
// }

/**
 * @brief          return the chassis 3508 motor data point
 * @param[in]      i: motor number,range [0,3]
 * @retval         motor data point
 */
/**
 * @brief          锟斤拷锟截碉拷锟教碉拷锟� 3508锟斤拷锟斤拷锟斤拷锟街革拷锟�
 * @param[in]      i: 锟斤拷锟斤拷锟斤拷,锟斤拷围[0,3]
 * @retval         锟斤拷锟斤拷锟斤拷锟街革拷锟�
 */
const Motor_Measure_T *get_chassis_motor_measure_point(uint8_t i)
{
  return &motor_chassis[(i & 0x03)];
}
